package es.uji.viselab.kinematics;

import es.uji.viselab.LogLevel;
import es.uji.viselab.LogRecorder;
import es.uji.viselab.math.Joints;
import es.uji.viselab.math.RobotToolLocation;

import org.jscience.mathematics.vector.Float64Matrix;

/**
 * Only having the DKinematics solved, this class uses that matrix to reach a
 * iterative solution (minimum error) through Jacobian.
 * 
 * @author roger
 * 
 */
public class IKinematic {

	static int targetItems = 3;
	static int numJoints = 6;
	int rows = numJoints;
	int columns = targetItems;

	/**
	 * 
	 * Iteration to minimize the error between targetRtl and the current RTL
	 * <p>
	 * The starting point to minimize that error is the current joints angles,
	 * from that position, and based on the Jacobian Matrix, I move the joints
	 * of the robot to reach the target RTL
	 * 
	 * @param joints
	 *            Current joints of the robot
	 * @param dk
	 *            DKinematic class that is capable to calculate the Direct
	 *            Kinematics, that is joints-->RobotToolLocation
	 * @param targetRtl
	 *            Request destination of the RobotToolLocation (position to
	 *            reach)
	 * @return null if it has not been posible to reach the position, or the
	 *         joints value otherwise
	 */
	public Joints iterate(Joints joints, DKinematic dk, RobotToolLocation targetRtl) {

		// A Derivate of a function is the slope in a certain point, or the
		// diference between the increment in y for a small increment in x. The
		// jacobianDerivateIncrement is the ammount of that increment
		double jacobianDerivateIncrement = 0.0001;

		// Remember this is loop that tries to reduce the error, next try for
		// joints value will be incremented this small amout
		double stepIncrement = 0.01;

		// Maximum steps of the loop
		int maxSteps = 100;

		// Maximum error allowed in milimeters (position) 
		// or in degrees (orientation requested)
		double maximumError = 1;

		// The Jacobian pseudo inverse will provide the increment we should apply
		double jointsIncrement[] = new double[numJoints];
		
		// The news values of joints to test
		double jointsStimation[] = new double[numJoints];
		
		// In each step, the new jointsStimation we will add the jointsIncrement
		// By now, the initial value, are the starting joints values. 
		jointsStimation =  joints.getArray();

		double original[] = new double[targetItems];
		double reached[] = new double[targetItems];
		double target[] = targetRtl.getArray();
		
		double targetDiference[] = new double[targetItems];

		original = dk.getRtl(joints).getArray();
		double error=999999;

		boolean exit = false;

		double[][] jacobianMatrix = new double[rows][columns];
		// JacobianInverse (or Jacobian Pseudo Inverse)
		double[][] jacobianInverse = new double[columns][rows];

		while (!exit) {
			// in each step, jointsStimation will (hope) go reaching more and more the target
			
			// Step 1. Calculation of Jacobian Pseudo-Inverse (JI)
			jacobianMatrix = getJacobian(dk, new Joints(jointsStimation), jacobianDerivateIncrement);
			jacobianInverse = getPseudoInverse(jacobianMatrix);
			
			// Step 2
			// Multiplication of the JI for the position error (diference)
			// Results in the Joints diference (increment)
			for (int i = 0; i < targetItems; i++)
				targetDiference[i] = target[i] - original[i]; 
			jointsIncrement = mult(jacobianInverse, targetDiference);

            // Step 3. New joints values, once applied the increment
			for (int i = 0; i < numJoints; i++)
				jointsStimation[i] = (jointsStimation[i] + jointsIncrement[i] * stepIncrement);

			// Step 4. Are the new joints value good ? let's calculate the new error
			reached = dk.getRtl(new Joints(jointsStimation)).getArray();
			error=0;
			for (int i = 0; i < targetItems; i++)
				if (i < 3)
					error = error + Math.abs(reached[i] - target[i]);
				else
					error = error; // TODO is is necessary to include the orientation in the loop
					// orientation
					// TODO error = error + 10 * Math.abs(reached[i] - target[i]);

			// Step 5. the position reached will be the original for next loop
			// The error will be calculated compared to this new position.
			for (int i = 0; i < targetItems; i++)
				original[i] = reached[i];
			
			maxSteps--;

			if (maxSteps == 0 || error < maximumError)
				exit = true;
		}
		if (error < maximumError)
			// Good, I return new joints value
			return new Joints(jointsStimation);
		else{
			LogRecorder.log(LogLevel.DEBUG,"Inverse kinematics failed");
			return joints; // old joints
		}
	}

	private double[][] getPseudoInverse(double[][] j) {
		Float64Matrix M = Float64Matrix.valueOf(j);
		M.pseudoInverse();
		// M.inverse();
		double[][] r = new double[columns][rows];
		for (int m = 0; m < rows; m++) {
			for (int n = 0; n < columns; n++) {
				r[n][m] = M.get(m, n).doubleValue();
			}
		}
		return r;
	}

	private double[] mult(double[][] jt, double[] v) {

		double result[] = new double[numJoints];
		for (int m = 0; m < numJoints; m++) {
			result[m] = 0;
			for (int n = 0; n < targetItems; n++) {
				result[m] = jt[n][m] * v[n] + result[m];
			}
		}
		return result;
	}

	public double[][] getJacobian(DKinematic dk, Joints joints, double increment) {

		double diference;
		double j0[] = new double[numJoints];
		double ja[] = new double[numJoints];
		double jb[] = new double[numJoints];

		j0 = joints.getArray();

		double[] e0 = new double[targetItems];
		double[] ea = new double[targetItems];
		double[] eb = new double[targetItems];

		RobotToolLocation rtl = null;
		// rtl = dk.getRtl(joints);
		// e0 = rtl.getArray();

		int rows = numJoints;// dk.getJoints();
		int columns = targetItems; // e: 3 for position (x,y,z), 3 for
									// orientation
		j0 = joints.getArray();
		double[][] a = new double[rows][columns];

		for (int m = 0; m < rows; m++) {
			for (int n = 0; n < columns; n++) {
				ja = joints.getArray();
				jb = joints.getArray();
				ja[m] = j0[m] + increment / 2;
				jb[m] = j0[m] - increment / 2;
				ea = dk.getRtl(new Joints(ja)).getArray();
				eb = dk.getRtl(new Joints(jb)).getArray();
				// variation of e(n) for the joint(m)
				diference = (ea[n] - eb[n]) / increment;
				a[m][n] = diference;
			}
		}
		return a;

	}

	public double[][] getTranspose(double a[][]) {

		int rows = numJoints;
		int columns = targetItems;

		double[][] t = new double[columns][rows];

		for (int m = 0; m < rows; m++) {
			for (int n = 0; n < columns; n++) {
				t[n][m] = a[m][n];
			}
		}
		return t;
	}

}
